﻿import smbus2,time,ctypes

class mpu_object:
    
    def __init__(self):
        self.bus = smbus2.SMBus(0)
        #固定参数
        self.MPU6050_ADDR = 0x68
        self.PWR_MGMT_1   = 0x6B
        self.SMPLRT_DIV   = 0x19
        self.CONFIG       = 0x1A
        self.GYRO_CONFIG  = 0x1B
        self.ACCEL_CONFIG = 0x1C
        self.INT_ENABLE   = 0x38
        self.ACCEL_XOUT_H = 0x3B
        self.ACCEL_YOUT_H = 0x3D
        self.ACCEL_ZOUT_H = 0x3F
        self.TEMP_OUT_H   = 0x41
        self.GYRO_XOUT_H  = 0x43
        self.GYRO_YOUT_H  = 0x45
        self.GYRO_ZOUT_H  = 0x47

        samp_rate_div = 7 # sample rate = 8 kHz/(1+samp_rate_div)
        self.bus.write_byte_data(self.MPU6050_ADDR,self.SMPLRT_DIV, samp_rate_div)

        self.bus.write_byte_data( self.MPU6050_ADDR, self.PWR_MGMT_1,0x00)
        time.sleep(0.1)
        self.bus.write_byte_data(self.MPU6050_ADDR, self.PWR_MGMT_1, 0x01)
        time.sleep(0.1)
        self.bus.write_byte_data( self.MPU6050_ADDR,  self.CONFIG, 0)
        time.sleep(0.1)

        #Write to Gyro configuration register
        gyro_config_sel = [0b00000,0b010000,0b10000,0b11000] # byte registers
        gyro_config_vals = [250.0,500.0,1000.0,2000.0] # degrees/sec
        gyro_indx = 0
        self.bus.write_byte_data( self.MPU6050_ADDR,  self.GYRO_CONFIG, int(gyro_config_sel[gyro_indx]))
        time.sleep(0.1)
        #Write to Accel configuration register
        accel_config_sel = [0b00000,0b01000,0b10000,0b11000] # byte registers
        accel_config_vals = [2.0,4.0,8.0,16.0] # g (g = 9.81 m/s^2)
        accel_indx = 0
        self.bus.write_byte_data( self.MPU6050_ADDR,  self.ACCEL_CONFIG, int(accel_config_sel[accel_indx]))
        time.sleep(0.1)
        self.gyro_sens = gyro_config_vals[gyro_indx]
        self.accel_sens=accel_config_vals[accel_indx]


        # x_accel_offset = int(-1588)
        # y_accel_offset = int(1520.875)
        # z_accel_offset = int(-755.5)
        # x_gyro_offset = int(16.5)
        # y_gyro_offset = int(-13.25)
        # z_gyro_offset = int(-25)
        # 
        # if x_accel_offset != 0:
        #     self.set_x_accel_offset(x_accel_offset)
        # 
        # if y_accel_offset != 0:
        #     self.set_y_accel_offset(y_accel_offset)
        # 
        # if z_accel_offset != 0:
        #     self.set_z_accel_offset(z_accel_offset)
        # 
        # if x_gyro_offset != 0:
        #     self.set_x_gyro_offset(x_gyro_offset)
        # 
        # if y_gyro_offset != 0:
        #     self.set_y_gyro_offset(y_gyro_offset)
        # 
        # if z_gyro_offset != 0:
        #     self.set_z_gyro_offset(z_gyro_offset)

    def read_raw_bits(self,register):
        # read accel and gyro values
        high = self.bus.read_byte_data(self.MPU6050_ADDR, register)
        low = self.bus.read_byte_data(self.MPU6050_ADDR, register+1)
    
        # combine higha and low for unsigned bit value
        value = ((high << 8) | low)
    
        # convert to +- value
        if(value > 32768):
            value -= 65536
        return value
    
    def read_accel(self):
        # read raw accel values
        x = self.read_raw_bits(self.ACCEL_XOUT_H)
        y = self.read_raw_bits(self.ACCEL_YOUT_H)
        z = self.read_raw_bits(self.ACCEL_ZOUT_H)
        
        scale = 32768/self.accel_sens
        
        x = x / scale
        y = y / scale
        z = z / scale
        return x,y,z

    def read_gyro(self):
        # read raw gyro values
        x = self.read_raw_bits(self.GYRO_XOUT_H)
        y = self.read_raw_bits(self.GYRO_YOUT_H)
        z = self.read_raw_bits(self.GYRO_ZOUT_H)
        
        scale = 32768/self.gyro_sens
        
        x = x / scale
        y = y / scale
        z = z / scale
        return x,y,z

    def set_x_accel_offset(self,a_offset):
        self.bus.write_byte_data(self.MPU6050_ADDR, 0x06,ctypes.c_int8(a_offset >> 8).value)
        self.bus.write_byte_data(self.MPU6050_ADDR, 0x07,ctypes.c_int8(a_offset).value)
    
    def set_y_accel_offset(self,a_offset):
        self.bus.write_byte_data(self.MPU6050_ADDR, 0x08,ctypes.c_int8(a_offset >> 8).value)
        self.bus.write_byte_data(self.MPU6050_ADDR, 0x09,ctypes.c_int8(a_offset).value)
    
    def set_z_accel_offset(self,a_offset):
        self.bus.write_byte_data(self.MPU6050_ADDR, 0x0A,ctypes.c_int8(a_offset >> 8).value)
        self.bus.write_byte_data(self.MPU6050_ADDR, 0x0B,ctypes.c_int8(a_offset).value)
    
    def set_x_gyro_offset(self,g_offset):
        self.bus.write_byte_data(self.MPU6050_ADDR, 0x13,ctypes.c_int8(g_offset >> 8).value)
        self.bus.write_byte_data(self.MPU6050_ADDR, 0x14,ctypes.c_int8(g_offset).value)
    
    def set_y_gyro_offset(self,g_offset):
        self.bus.write_byte_data(self.MPU6050_ADDR, 0x15,ctypes.c_int8(g_offset >> 8).value)
        self.bus.write_byte_data(self.MPU6050_ADDR, 0x16,ctypes.c_int8(g_offset).value)
    
    def set_z_gyro_offset(self,g_offset):
        self.bus.write_byte_data(self.MPU6050_ADDR, 0x17,ctypes.c_int8(g_offset >> 8).value)
        self.bus.write_byte_data(self.MPU6050_ADDR, 0x18,ctypes.c_int8(g_offset).value)
